Robust Ego-Motion Estimation with ToF Cameras

This paper presents an approach to estimate the ego-motion of a robot while moving. The employed sensor is a Time-of-Flight (ToF) camera, the SR3000 from Mesa Imaging. ToF cameras provide depth and reflectance data of the scene at high frame rates. The proposed method utilizes the coherence of depth and reflectance data of ToF cameras by detecting image features on reflectance data and estimating the motion on depth data. The motion estimate of the camera is fused with inertial measurements to gain higher accuracy and robustness. The result of the algorithm is benchmarked against reference poses determined by matching accurate 2D range scans. The evaluation shows that fusing the pose estimate with the data from the IMU improves the accuracy and robustness of the motion estimate against distorted measurements from the sensor.